#include <Arduino.h>
#include <U8g2lib.h>
#include <MPU9250.h>

#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif


U8G2_SH1107_64X128_F_4W_HW_SPI u8g2(U8G2_R0, /* cs=*/14, /* dc=*/27, /* reset=*/33);

MPU9250 mpu;

int   x, y, z;
int   cal_x = 0;    // X修正值
int   cal_y = 0;    // Y修正值

// End of constructor list

void DisplayString(const char* text)
{
  u8g2.clearBuffer();

  u8g2.drawStr(10, 55, text);
  u8g2.sendBuffer();
}

void DrawLevel(int accX, int accY)
{
  int   x_offset, y_offset;
  char  strBuf[10];

  // 修正
  accX += cal_x;
  accY += cal_y;

  u8g2.clearBuffer();

  u8g2.drawCircle(32, 64, 30);
  u8g2.drawCircle(32, 64, 13);
  u8g2.drawLine(32, 32, 32, 96);
  u8g2.drawLine(1, 64, 63, 64);

  sprintf(strBuf, "%d", accX);
  u8g2.drawStr(8, 20, strBuf);
  sprintf(strBuf, "%d", accY);
  u8g2.drawStr(8, 120, strBuf);

  u8g2.drawFrame(0, 2, 64, 24);
  u8g2.drawLine(0, 26, 0, 64);

  u8g2.drawFrame(0, 102, 64, 24);
  u8g2.drawLine(32, 102, 32, 96);

  // u8g2.drawDisc(32, 64, 10);

  x_offset = (accX / 10) + 32;
  if (x_offset > 63) {
    x_offset = 63;
  }
  if (x_offset < 0) {
    x_offset = 0;
  }

  y_offset = (accY / 10) + 32;
  if (y_offset > 63) {
    y_offset = 63;
  }
  if (y_offset < 0) {
    y_offset = 0;
  }

  u8g2.drawDisc(x_offset, y_offset + 32, 10);

  u8g2.sendBuffer();
}

void print_acc() {
    Serial.print("X, Y, Z: ");
    Serial.print(mpu.getAccX(), 3);
    Serial.print(", ");
    Serial.print(mpu.getAccY(), 3);
    Serial.print(", ");
    Serial.println(mpu.getAccZ(), 3);
}

void setup(void)
{
  Serial.begin(115200);
  Wire.begin();
  delay(1000);

  MPU9250Setting setting;
  setting.accel_fs_sel = ACCEL_FS_SEL::A2G;
  setting.gyro_fs_sel = GYRO_FS_SEL::G2000DPS;
  setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;
  setting.fifo_sample_rate = FIFO_SAMPLE_RATE::SMPL_200HZ;
  setting.gyro_fchoice = 0x03;
  setting.gyro_dlpf_cfg = GYRO_DLPF_CFG::DLPF_41HZ;
  setting.accel_fchoice = 0x01;
  setting.accel_dlpf_cfg = ACCEL_DLPF_CFG::DLPF_5HZ;

  mpu.setup(0x68, setting);
  x = y = z = 0;

  u8g2.begin();
  u8g2.setContrast(200);
  // u8g2.setFont(u8g2_font_t0_14_mr);
  u8g2.setFont(u8g2_font_courB14_tr);

  // DisplayString("test");
  DrawLevel(0, 0);

}

void loop(void)
{
  // u8g2.firstPage();
  // do
  // {
  //   u8g2.setFont(u8g2_font_ncenB10_tr);
  //   u8g2.drawStr(0, 24, "Hello World!");
  // } while (u8g2.nextPage());
  // delay(1000);

  delay(500);

  if (mpu.update())
  {
    x = mpu.getAccX() * 10000;
    y = mpu.getAccY() * 10000;
    z = mpu.getAccZ() * 10000;

    Serial.print("X, Y, Z: ");
    Serial.print(x);
    Serial.print(", ");
    Serial.print(y);
    Serial.print(", ");
    Serial.println(z);

    DrawLevel(x * -1, y);
  }
}
